// --------------------------------------------------------------------------------------------------------------------
// <copyright file="HorizontalDepthProfileModel.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Visualization
{
    using System;
    using System.Collections.Generic;
    using System.Windows.Media;
    using System.Windows.Media.Imaging;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Visualization;

    /// <summary>
    /// Model for visualizing a top-down view of a depth image
    /// </summary>
    public sealed class HorizontalDepthProfileModel : Model
    {
        /// <summary>
        /// Byte offset of blue pixel
        /// </summary>
        private const int BlueByteOffset = 0;

        /// <summary>
        /// Byte offset of green pixel
        /// </summary>
        private const int GreenByteOffset = 1;

        /// <summary>
        /// Byte offset of red pixel
        /// </summary>
        private const int RedByteOffset = 2;

        /// <summary>
        /// Byte offset of red pixel
        /// </summary>
        private const int AlphaByteOffset = 3;

        /// <summary>
        /// Number of bytes per pixel
        /// </summary>
        private const int BytesPerPixel = 4;

        /// <summary>
        /// The cutoff depth for observed distances (along the ray)
        /// </summary>
        private double maximumDepth;

        /// <summary>
        /// The resolution of a pixel in the visualization
        /// </summary>
        private double resolutionInMetersPerPixel;

        /// <summary>
        /// The internal back-buffer
        /// </summary>
        private byte[] rawImage;

        /// <summary>
        /// The width of the visualization bitmap
        /// </summary>
        private int width;

        /// <summary>
        /// The height of the visualization bitmap
        /// </summary>
        private int height;

        /// <summary>
        /// Initializes a new instance of the HorizontalDepthProfileModel class.
        /// </summary>
        public HorizontalDepthProfileModel()
        {
        }

        /// <summary>
        /// Gets the video frame representing the horizontally projected depth image
        /// </summary>
        public VideoFrame VideoFrame { get; private set; }

        /// <summary>
        /// Updates the model with the provided hdp.
        /// </summary>
        /// <param name="hdp">Depth profile to visualize</param>
        /// <param name="maximumDepth">The cutoff depth for observed distances along the ray</param>
        /// <param name="resolutionInMetersPerPixel">The resolution of a visualization pixel</param>
        /// <param name="obstacleColor">The color for obstacles</param>
        /// <param name="openSpaceColor">The color to use for open space (in front of obstacles)</param>
        /// <param name="unexploredSpaceColor">The color to use for unexplored space (no readings or beyond obstacles)</param>
        public void UpdateWithHdp(IHorizontalDepthProfile hdp, double maximumDepth, double resolutionInMetersPerPixel, Color obstacleColor, Color openSpaceColor, Color unexploredSpaceColor)
        {
            if (this.rawImage == null || this.maximumDepth != maximumDepth || this.resolutionInMetersPerPixel != resolutionInMetersPerPixel)
            {
                // re-initialize
                this.maximumDepth = maximumDepth;
                this.resolutionInMetersPerPixel = resolutionInMetersPerPixel;
                this.width = (int)(hdp.GetFOVWidthForDepth(this.maximumDepth) / this.resolutionInMetersPerPixel);
                this.height = (int)(this.maximumDepth / this.resolutionInMetersPerPixel);
                this.rawImage = new byte[this.height * this.width * BytesPerPixel];
            }

            int index = 0;
            int halfWidth = this.width / 2;
            for (int x = 0; x < this.height; x++)
            {
                for (int y = 0; y < this.width; y++)
                {
                    // reproject the pixel from the plane into the hdp
                    double distance;
                    int rayIndex = hdp.Reproject(new Point2D(x * this.resolutionInMetersPerPixel, (y - halfWidth) * resolutionInMetersPerPixel), out distance);
                    if (rayIndex >= 0)
                    {
                        distance = hdp.GetHitDistance(rayIndex) - distance;
                    }
                    else
                    {
                        distance = 0;
                    }

                    Color pixelColor = unexploredSpaceColor;

                    if (distance > this.resolutionInMetersPerPixel)
                    {
                        pixelColor = openSpaceColor;
                    }

                    index = x * this.width + y;
                    this.SetPixel(index, pixelColor);
                }
            }

            // now display the hit points
            int count = hdp.Length;
            for (int i = 0; i < count; i++)
            {
                Point2D point = hdp.GetHitPoint(i);
                if (point.X == 0 && point.Y == 0)
                {
                    continue;
                }

                int x = (int)(point.X / this.resolutionInMetersPerPixel + 0.5);
                if (x >= this.height)
                {
                    continue;
                }

                int y = (int)(point.Y / this.resolutionInMetersPerPixel + halfWidth + 0.5);
                if (y >= this.width || y < 0)
                {
                    continue;
                }

                index = x * this.width + y;
                this.SetPixel(index, obstacleColor);
            }
            
            this.VideoFrame = new VideoFrame()
            {
                Data = this.rawImage,
                Format = SimplePixelFormat.Bgra32,
                Stride = this.width * BytesPerPixel,
                Size = new System.Drawing.Size(this.width, this.height)
            };
        }

        /// <summary>
        /// Sets the color of the pixel to the specified value
        /// </summary>
        /// <param name="index">The index of the pixel</param>
        /// <param name="pixelColor">The color</param>
        private void SetPixel(int index, Color pixelColor)
        {
            this.rawImage[index * BytesPerPixel + AlphaByteOffset] = pixelColor.A;
            this.rawImage[index * BytesPerPixel + RedByteOffset] = pixelColor.R;
            this.rawImage[index * BytesPerPixel + GreenByteOffset] = pixelColor.G;
            this.rawImage[index * BytesPerPixel + BlueByteOffset] = pixelColor.B;
        }
    }
}
